\section{Распространение фильтра Калмана на нелинейные задачи}
	
Фильтр Калмана предназначен для гауссовских линейных систем. В данном параграфе мы разберем
как быть в нелинейномn случае. Рассматривается система:
$$
\left\{
\begin{aligned}
	x_{k+1} &= f_k(x_k, w_k) & &\text{ --- стохастическое разностное ур--е динамики}
	\\
	z_{k} &= h_k(x_k, v_k) & &\text{ --- уравнение наблюдения}
\end{aligned}
\right.
$$
где решением уравнения динамики является {\it мартингал}.

\par Будем работать в пространстве $\mathcal{L}_2$, где любому вектору ставится в соответствие
некоторое
гауссовское распределение (полагаем, что помехи гауссовские). Введем стандартные предположения и 
обозначения:
\begin{gather*}
	\E w_k = \E v_k = 0
	\\
	M_k = \D w_k, \quad N_k = \D v_k
	\\
	x_0, w_k, v_k \text{ --- независимы в совокупности}
\end{gather*}		 
При этом распределение $x_0$ абсолютно непрерывно и задано через плотность 
$p(x_0)$\footnote{Распределение $x_0$ не обязательно гауссовское}. Таким образом, имеем 
$p(x_0 \condp z_0) = p(x_0)$.
\begin{problem}
	$p(x_k \condp Z_k) = \: ?$, \; $Z_k = \{z_k, z_{k - 1}, \ldots, z_0\}$
\end{problem}

\par Как и в линейном случае, эта задача разбивается на два этапа:
\begin{enumerate}
	\item \underline{Prediction} \quad уравнение Колмогорова--Чепмена:
	\begin{equation}
		p(x_k \condp Z_{k - 1}) = \int p(x_k \condp x_{k - 1}) p(x_{k - 1} \condp Z_{k - 1}) 
		\, dx_{k - 1},
	\end{equation}
	где $p(x_k \condp x_{k - 1}, Z_{k - 1}) = p(x_k \condp x_{k - 1})$ в силу мартингального 
	свойства			 
	\item \underline{Correction} \quad формула Байеса:
	\begin{equation}
		p(x_k \condp Z_k) = p(x_k \condp z_k, Z_{k - 1}) = \frac{p(z_k \condp x_k) 
		p(x_k \condp Z_k)}{\int p(z_k \condp x_k) p(x_k \condp Z_{k - 1}) \, dx_k}
	\end{equation}
\end{enumerate}

\par Если мы хотим получить {\it точечную оценку} состояния $x_k$, то можно в качестве такой оценки взять
моду или математическое ожидание.
Причём, если распределение $x_0$ гауссовское, то эти оценки совпадут.
\subsection{Аналитическое решение в трёх классах}
В линейно--гауссовском случае это обыкновенный фильтр Калмана.

\subsubsection*{Дискретный случай с конечным числом состояний}

Здесь $x_k, \, k \in \{1, \ldots, N\}$ пробегает конечное число значений.
Вводим 
\begin{equation}
	\omega_{k - 1 \condp k - 1}^i = \pr(x_{k - 1} = x_{k - 1}^i \condp Z_{k - 1})
\end{equation}
--- вероятность того, что в $k - 1$ момент времени 	система находится в $i$--ом состоянии.
Тогда имеем выражение для плотности:
\begin{gather}
	p(x_{k - 1} \condp Z_{k - 1}) = \sum\limits_{i = 1}^N \omega_{k - 1 \condp k - 1}^i	
	\, \delta(x_{k - 1} - x_{k - 1}^i)
\end{gather}
\begin{enumerate}
	\item \underline{Prediction}
	\begin{equation}
		p(x_k \condp Z_{k - 1}) = \sum\limits_{i = 1}^N \omega_{k \condp k - 1}^i
		\, \delta(x_k - x_k^i)
	\end{equation}
	\item \underline{Correction}
	\begin{equation}
		p(x_k \condp Z_k) = \sum\limits_{i = 1}^N \omega_{k \condp k}^i \, \delta(x_k - x_k^i)
	\end{equation}
	где 
	\begin{gather}
		\omega_{k \condp k - 1}^i = \sum\limits_{j = 1}^N \omega_{k - 1 \condp k - 1}^j
		\, p(x_k^i \condp x_{k - 1}^j)
		\\
		\omega_{k \condp k}^i = \frac{\omega_{k \condp k - 1}^i \, p(z_k \condp x_k^i)}
		{\sum\limits_{j = 1}^N \omega_{k \condp k - 1}^j \, p(z_k \condp x_k^j)}
	\end{gather}
\end{enumerate}

\subsubsection*{Нелинейность специального вида}

Здесь уравнение измерения линейно. Имеем:
\begin{equation}
	\left\{
	\begin{aligned}
		dx_t &= f(x_t) \, dt + dw_t
		\\
		y_k &= H_k x_k + v_k
	\end{aligned}
	\right.
\end{equation}
Рассмотрим подробнее функцию $f$, на нее накладывается следующее условие:
\begin{equation}
	\label{cond_f}
	\tr \left(\nabla_x f^\T\right) + f^\T f = x^\T Ax + B^\T x + C
\end{equation}
Напомним, что $\nabla_x = \left( \pdiff{}{x_1}, \ldots, \pdiff{}{x_n}\right)$ и,
следовательно, $\tr \left( \nabla_x f^\T \right) = 
\left( \pdiff{f_1}{x_1}, \ldots, \pdiff{f_n}{x_n}\right)$.

\begin{ex}
	{\it Saturation Phenomenon \footnote{Модель насыщения}}
	\par 
	Функция, удовлетворяющая условию \eqref{cond_f}:
	\begin{equation}
		f(x) = \frac{Ae^x - Be^{-x}}{Ae^x + Be^{-x}}
	\end{equation}
	\par Если $A = B$, то $f(x) = \th x \; \Goes \; x^\T A x + B^\T x + C = 1$ 
\end{ex}

Теперь пусть на отрезке $[0, t]$ произведено $N$ наблюдений $0 \leq t_1 \leq 
\ldots \leq t_n \leq t$. На промежутке $[t_{k - 1}, t_k]$ процесс описывается с помощью
вектора математических ожиданий $m_t$ и матрицы дисперсии $P_t$. 

\begin{enumerate}
	\item \underline{Prediction}
		\begin{equation}
			\left\{
			\begin{aligned}
				\dot{m}_t &= -P_t A m_t - \frac12 P_t B
				\\
				m_{t_{k - 1}} &= m_{k - 1}
			\end{aligned}
			\right.
			\quad
			\left\{
			\begin{aligned}
				\dot{P}_t &= \mOne - P_t A P_t
				\\
				P_{t_{k - 1}} &= P_{k - 1}
			\end{aligned}
			\right.
		\end{equation}
		\par Из этого следует $m_{t_k - 0} = \bar{m}_k, \, P_{t_k - 0} = \bar{P}_k$.
	\item \underline{Correction}
		\begin{equation}
			\left\{
			\begin{aligned}
				P_k &= \bar{P}_k - \bar{P}_k H_k (H_k \bar{P}_k H_k + N_k)^{-1}H_k\bar{P}_k
				\\
				m_k &= \bar{m}_k + P_k H_k^\T R_k^{-1} (Z_k - H_k \bar{m}_k)
			\end{aligned}
			\right.
		\end{equation}
\end{enumerate}

\subsection{Приближенные методы}
\subsubsection*{Extended Kalman Filter}

Этот метод применяется к системам следующего вида:
\begin{equation}
	\label{ext_cal}
	\begin{cases}
		x_k = f_{k - 1}(x_{k - 1}) + w_{k - 1}
		\\
		z_k = h_k(x_k) + v_k
	\end{cases}
\end{equation}
где $f_k, h_k$ --- непрерывно дифференцируемы, при этом нас устаивает их локальная
линейная аппроксимация

\begin{enumerate}
	\item \underline{Prediction}
		\begin{gather}
			\left\{
			\begin{aligned}
				\hat{x}_{k \condp k - 1} &= f_{k - 1} (\hat{x}_{k - 1\condp k - 1})
				\\
				R_{k \condp k - 1} &= M_{k - 1} + \hat{F}_{k - 1} R_{k - 1 \condp k - 1} 
				\hat{F}_{k - 1}^\T
			\end{aligned}
			\right.
			\\
			\notag
			\text{где } \left. \hat{F}_{k - 1} = 
			\left[ \nabla_x f_{k - 1}^\T(x_{k - 1}) \right]^\T
			\right|_{
			x_{k - 1} = \hat{x}_{k - 1 \condp k - 1}} 
		\end{gather}	
	\item \underline{Correction}
		\begin{gather}
			\left\{
			\begin{aligned}
				\hat{x}_{k \condp k} &= \hat{x}_{k \condp k - 1} + R_{k \condp k - 1} \hat{H}_k^\T
				\left(\hat{H}_k R_{k \condp k - 1} \hat{H}_k^\T + N_k\right)^{-1} 
				\\
				R_{k \condp k} &= M_k + R_{k \condp k - 1} \hat{H}_k^\T 
				\left(\hat{H}_k R_{k \condp k - 1} \hat{H}_k^\T + N_k\right)^{-1}
				\hat{H}_k R_{k \condp k - 1}
			\end{aligned}
			\right.
			\\
			\notag
			\text{где }  \hat{H}_k = \left. 
			\left[ \nabla_x h_k^\T x_k \right]^\T
			\right|_{x_k = \hat{x}_{k \condp k - 1}}
		\end{gather}
\end{enumerate}
\subsubsection*{Численная аппроксимация}

При очень сложной нелинейности можно разбить все простанство состояний на ячейки и из каждой
ячейки выбрать представителя (например, чебышевский центр). Далее задача сводится к задаче с
конечным числом состояний.

\subsubsection*{Сумма гауссовских распределений}

Популярный в статистике метод пытается вычленить в плотности отдельные гауссовские составляющие.
Пусть $p(x_k \condp Z_k)$ --- мультимодальная функция (на систему действуют несколько гауссовских 
помех), то можно аппроксимировать (это апостериорная плотность, ее оценивают по гистограммам):

\begin{equation}
	p(x_k \condp Z_k) \approx \sum \limits_{i = 1}^{q_k} \omega_k^i  \,
	\mathcal{N}\!\left( x_k^i; \,\hat{x}_{k \condp k}^i, R_{k \condp k}^i \right)
\end{equation}
где $\omega_k^i$ --- веса: $\sum\limits_{i = 1}^{q_k} \omega_k^i = 1, \, \omega_k^i \geq 0$,
$q_k$ --- количество локальных максимумов $p(x_k \condp Z_k)$.
У метода есть статический и динамический варианты.

\subsection{Модель с переключениями, методы отбрасывания и перемешивания}

Рассмотрим подробно методы приближения суммой гауссовских случайных величин.

\subsubsection{SMME}

Статический мультимодальный оцениватель.
\par В статическом случае $q_k = S$ --- некоторой константе. По фильтру Калмана находим
$\hat{x}_{k \condp k}, R_{k \condp k}^i, \, i = 1, \ldots, S$. Теперь требуется найти формулу для
весов:
\begin{gather}
	\omega_k^i = \pr(i \condp Z_k) = \pr(i \condp Z_{k - 1}, z_k) = \text{\{ф-ла Байеса\}}
	= \frac{\pr(z_k \condp Z_{k - 1}, i) \pr(i \condp Z_{k - 1})}{\pr(z_k \condp Z_{k - 1})}
	\intertext{так как $\pr(z_k \condp Z_{k - 1}) = 
	\sum\limits_{i = 1}^s \pr(z_k \condp Z_{k - 1}, i) \pr (i \condp Z_{k - 1})$,
	\text{ а } $\pr(i \condp Z_{k - 1}) = \omega_{k - 1}^i$, \text{ то:}}
	\omega_k^i = \frac{\pr(z_k \condp Z_{k - 1}, i) \, \omega_{k - 1}^i}
	{\sum\limits_{i = 1}^s \pr(z_k \condp Z_{k - 1}, i) \, \omega_{k - 1}^i},
\end{gather}	
\text{где } $\pr(z_k \condp Z_{k - 1}, i) \approx 
	\mathcal{N}\!\left(v_k^i; \, 0, S_k^i\right)$, \;
	$v_k^i$ \text{ --- помеха, } $\E v_k^i = 0, \D v_k^i = S_k^i$, при этом справедливо выражение
	$\bar{z}_k = C_k^i \bar{x}_{k \condp k} + v_k^i$
\par Теперь мы готовы выписать оценочные формулы:
\begin{equation}
	\left\{
	\begin{aligned}
		\bar{x}_{k \condp k} &= \sum\limits_{i = 1}^S \omega_k^i \hat{x}_{k \condp k}^i
				\\
		R_{k \condp k} &= \sum\limits_{i = 1}^S \omega_k^i \bigg(
		R_{k \condp k}^i + \underbrace{\left(\hat{x}_{k \condp k}^i - \bar{x}_{k \condp k}\right)
		\left( \hat{x}_{k \condp k}^i - \bar{x}_{k \condp k}\right)^\T}_\text{
		ошибка мат. ожидания}
		\bigg)
	\end{aligned}
	\right.
\end{equation}

\subsubsection*{DMME}
Динамический мультимодальный оцениватель.
\par Рассмотрим для начала общую модель (она применительна и к {\tt SMME}):
\begin{equation}
	\begin{cases}
		x_k = f_{k - 1}(x_{k - 1}, r_k, w_{k - 1})
		\\
		z_k = h_k(x_k, r_k, v_k), &k = 1 \ldots S
	\end{cases}
\end{equation}
где $r_k$ --- режим, $r_k = \const$ на $(t_{k - 1}, t_k]$. Обозначим за $t_k^+$ скачок с $r_k$
на $r_{k + 1}$ при общем числе скачков $S$. Обозначим:
\begin{gather}
	\pi_{ij} = \pr (r_k = j \condp r_{k - 1} = i), \;i, j = \overline{1, S}
	\quad
	\pi_{ij} \geq 0, \; \sum\limits_{j = 1}^S \pi_{ij} = 1
	\\
	\mu_i = \pr(r_0 = i), \; i = \overline{1, S} \quad
	\mu_i \geq 0, \; \sum\limits_{i = 1}^S \mu_i = 1
\end{gather}
\par Расчетные формулы на отрезке $(t_{k - 1}, t_k]$ принимают вид:
\begin{enumerate}
	\item \underline{Prediction}
		\begin{equation}
			p(x_k, r_k = j \condp Z_{k - 1}) = \sum\limits_{i = 1}^S
			\pi_{ij} \int p(x_k \condp x_{k - 1}, r_k = j) \, p(x_{k - 1}, r_{k - 1} = i) 
			\condp Z_{k - 1}) \, dx_{k - 1}
		\end{equation}
	\item \underline{Correction}
		\begin{equation}
			p(x_j, r_k = j \condp Z_k) =
			\frac{p(z_k \condp x_k, r_k = j) \, p(x_k, r_k = j \condp Z_{k - 1})}
			{ \sum\limits_{j = 1}^S \int p(z_k \condp x_k, r_k = j) \, 
			p(x_k, r_k = j \condp Z_{k - 1}) \, dx_k}
		\end{equation}
\end{enumerate}
\par Частным случаем этого оценивателя является {\tt JMLS} --- \glqq jump Markov 
linear systems\grqq{}:
\begin{equation}
	\begin{cases}
		x_k = F_{k - 1}(r_{k - 1})x_{k - 1} + w_{k - 1}(r_{k - 1})
		\\
		z_k = H_k(r_k)x_k + v_k(r_k)
	\end{cases}
\end{equation}	
где $w_k, v_k$ --- гауссовские шумы. Это дискретная модель, следовательно, в формулах будут
фигурировать суммы. Обозначив предысторию изменения режимов $R_k^l = \{r_1^l, \ldots, 
r_k^l\}$, имеем:
\begin{equation}
	\left\{
	\begin{aligned}
		p(x_k \condp Z_k) &= \sum\limits_{l = 1}^{S^k} p(x_k \condp R_k^l, Z_k)
		\pr (R_k^l \condp Z_k)
		\\
		\pr(R_k^l \condp Z_k) &= \pr(R_k^l \condp z_k, Z_{k - 1}) = \text{\{ф--ла Байеса\}} =
		\frac{p(z_k \condp R_k^l, Z_{k - 1}) \pr(R_k^l \condp Z_{k - 1})}
		{\sum\limits_{l = 1}^{S^k} p(z_k \condp R_k^l, Z_{k - 1}) \pr(R_k^l \condp Z_{k - 1})}
	\end{aligned}
	\right.
\end{equation}

\par {\tt DMME} подразделяется на два типа:
\par --- {\tt MMP} (\glqq prime\grqq). Поведение заключается в отбрасывании наименее популярных 
решений. Распараллеливается по $i$.
\par --- {\tt IMM} (\glqq integrated\grqq). Здесь мы перемешиваем и усредняем возможные варианты.
Не распараллеливается.

\subsubsection*{DMME MMP}
\par Предположим, что число пиков фиксировано $q_k := M$\footnote{Присваивание означает,
что мы полагаем}. Тогда:
\begin{gather}
	\omega_{k - 1}^l := \pr(R_{k - 1} \condp Z_{k - 1}), \lefteqn{\quad l = 1, \ldots, M}
	\\
	\omega_k^{l, i} := \pr(r_k = i, R_{k - 1}^l \condp Z_k) = 
	\frac{p(z_k \condp Z_{k - 1}, r_k = i, R_{k - 1}^l) \pr (r_k = i \condp R_{k - 1}^l)
	\pr (R_{k - 1}^l \condp Z_{k - 1})}
	{p(z_k \condp Z_{k - 1})}
\end{gather}
\par В данном случае $\pi_{R_{k - 1}^l, i} = \pr (r_k = i \condp R_{ k -1}^l)$
\par Всего $\omega_k^{l, i}$ $S \cdot M$ штук. Из них $M$ наиболее вероятных оставляем, а потом
производим нормировку $\sum_{1\ldots M} \omega_k^{l, i} = 1$

\subsubsection*{DMME IMM}
Здесь нас интересует лишь текущие вероятности перехода из одного состояния в другое (без 
предыстории). Вероятность перехода из $j$ в $i$:
\begin{gather}
	\mu_{k - 1}^{i \condp j} = \pr (r_{k - 1} = i \condp r_k = j, Z_{k - 1}) = 
	\frac{\pi_{ij} \, \omega_{k - 1}^i}
	{\sum\limits_{i = 1}^S \pi_{ij} \, \omega_{k - 1}^i}
	\\
	\notag
	\pi_{ij} = \pr(r_k = j \condp r_{j - 1} = i) \quad 
	\omega_{k - 1}^i = \pr (r_{k - 1} = i \condp Z_{k - 1})
\end{gather}
\par И тогда формулы фильтра примут вид:
\begin{equation}
	\left\{
	\begin{aligned}
		\bar{x}_{k - 1 \condp k - 1}^j &:= \sum\limits_{i = 1}^S \mu_{k - 1}^{i \condp j}
		\bar{x}_{k - 1 \condp k - 1}^i
		\\
		R_{k - 1 \condp k - 1}^j &:= \sum\limits_{i = 1}^S \mu_{k - 1}^{i \condp j}
		\left( 
		R_{k - 1 \condp k - 1}^i + 
		\left( \bar{x}_{k - 1 \condp k - 1}^i - \bar{x}_{k - 1 \condp k - 1}^j\right)		
		\left( \bar{x}_{k - 1 \condp k - 1}^i - \bar{x}_{k - 1 \condp k - 1}^j\right)^\T
		\right)
	\end{aligned}
	\right.
\end{equation}
А пересчет весов будет выглядеть следующим образом:
\begin{equation}
	\omega_k^j := \frac{p(z_k \condp Z_{k - 1}, r_k = j)
	\sum\limits_{i = 1}^S \pi_{ij}\,\omega_{k - 1}^i
	}	
	{\sum\limits_{j = 1}^S p(z_k \condp Z_{k - 1}, r_k = j) 
	\sum\limits_{i = 1}^S \pi_{ij} \,\omega_{k - 1}^i} = \pr(r_k = j \condp Z_k)
\end{equation}

\subsection{Численная аппроксимация}
Вернемся к уравнению \eqref{ext_cal}.
\par Пусть получено $p(x_{k - 1} \condp Z_{k - 1}) \approx \mathcal{N}\!(x_{k - 1}; \, 
\bar{x}_{k - 1 \condp k - 1}, R_{k - 1 \condp k - 1})$. Состояние $(x_{k - 1}^i, 
\omega_{k - 1}^i)$ каким-то образом выбрано.
\begin{enumerate}
	\item \underline{Prediction}
		\begin{equation}
			\left\{
			\begin{aligned}
				\bar{x}_{k \condp k - 1} &= \sum\limits_{i = 0}^{N - 1} \omega_{k - 1}^i \,
				f_{k - 1}(x_{k - 1}^i)
				\\
				R_{k \condp k - 1} &= M_{k - 1}  + \sum\limits_{i = 0}^{N - 1} \omega_{k - 1}^i
				\left(
				\, f_{k - 1}(x_{k - 1}^i) - \bar{x}_{k \condp k - 1}
				\right)
				\left(
				f_{k - 1}(x_{k - 1}^i) - \bar{x}_{k \condp k - 1}
				\right)^\T
			\end{aligned}
			\right.
		\end{equation}
		\par $p(x_k \condp Z_{k - 1})$ вычисляется аналогично (замена $k - 1$ на $k$).
		Усредненное наблюдение имеет вид:
		\begin{equation}
			\bar{z}_{k \condp k - 1} = \sum\limits_{i = 0}^{N - 1} \omega_{k - 1}^i \,
			h_k(x_{k - 1}^i)
		\end{equation}
	\item \underline{Correction}
		\begin{equation}
			\left\{
			\begin{aligned}
				\bar{x}_{k \condp k} &= \bar{x}_{k \condp k - 1} + K_k(z_k - \bar{z}_{k \condp k - 1})
				\\
				R_{k \condp k} &= R_{k \condp k - 1} - K_k S_k K_k^\T
			\end{aligned}
			\right.
		\end{equation}
		где
		\begin{gather*}
			K_k = P_{xz}N_k^{-1}, \; S_k = R_{k \condp k - 1} + P_{zz}
			\\
			P_{xz} = \sum\limits_{i = 0}^{N - 1} \omega_{k - 1}^i
			\left(
			x_{k \condp k - 1}^i - \bar{x}_{k \condp k - 1}
			\right)
			\left(
			h(x_{k \condp k - 1}^i - \bar{z}_{k \condp k - 1})
			\right)^\T
			\\
			P_{zz} = \sum\limits_{i = 0}^{N - 1} \omega_{k - 1}^i
			\left(
			z_{k \condp k - 1}^i - \bar{z}_{k \condp k - 1}
			\right)
			\left(
			h(x_{k \condp k - 1}^i - \bar{z}_{k \condp k - 1})
			\right)^\T
		\end{gather*}
\end{enumerate}
\par Теперь нам нужно понять, как выбирать пару $(x_k^i, \omega_k^i)$.
\par Пусть дан случайный вектор $a$ и $\bar{a}, R_a$ --- первый и второй его моменты. Пусть
задано отображение $g\colon a \goes b$, т.е. $b = g(a)$.
\par Предположим, что $(n_a + K)R_a = L^\T L$, $n_a$ --- кол--во
точек, и выберем $(A_i, \omega_i)$ по следующему правилу:
\begin{align*}
	A_0 &= \bar{a}
	\\
	A_i &= \bar{a} + \underbrace{\left( \sqrt{(n_a + K)R_a}\right)_i}_{\text{$i$--ая строка матрицы $L$}}
	\\
	A_i &= \bar{a} - \left( \sqrt{(n_a + K)R_a}\right)_i, \quad i = n_a + 1, \ldots, 2n_a
\end{align*}
при этом всего $1 + 2n_a$ точек разбиения. Веса при этом равны
\begin{align*}
	\omega_0 &= \frac{K}{n_a - K}, \quad K \neq n_a
	\\
	\omega_i &= \frac{1}{2(n_a + K)}
\end{align*}
\par Теперь посчитаем моменты вектора $b$ из отображения:
\begin{gather}
	\bar{b} = \sum\limits_{i = 0}^{2n_a} \omega_i \, B_i
	\\
	R_b = \sum\limits_{k = 0}^{n_a}\omega_i (B_i - \bar{b})(B_i - \bar{b})^\T
	\\
	\notag
	B_i = g(A_i)
\end{gather}